/* @brief An experimental demo to test my PID code.
 * @note This relys on tested BLDC utilities.
 */

#include <BLDC.h>
#include <pid.h>


bool TimeUp(){
  static const unsigned int interval = 500;
  static long lastTime = millis();
  if (millis() - lastTime > interval) {
    lastTime = millis();
    return true;
  }
  return false;
}


String inString;
PID pid;


void setup() {
  Serial.begin(57600);
  BLDC_Init();
  pid.kp = 2;
  pid.ki = 1;
}


void loop() {
  BLDC_Update();

  if (TimeUp()) {
    float _speed = BLDC_ReadSpeed();
    pid.Update(_speed);
    
    Serial.print(_speed); Serial.print(" ");
    Serial.println(pid.out);
    if (0 > pid.out) {
      BLDC_SetPwmDuty(0);
    }
    else {
      BLDC_SetPwmDuty(pid.out);
    }
  }

  // Read input and set speed:
  while (Serial.available() > 0) {
    int inChar = Serial.read();
    if (isDigit(inChar)) {
      inString += (char)inChar;
    }
    if (inChar == '\n') {
      pid.target = inString.toFloat();
      Serial.print("set target to");
      Serial.println(pid.target);
      inString = "";
    }
  }
}
